/**
  ******************************************************************************
  * File Name          : teleop_joy.cpp
  * Description       : teleop_joy program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <teleop_joy/teleop_joy.h>

using namespace std;
 
class Teleop
{
public:
    Teleop();
 
private:
    /* data */
    void callback(const sensor_msgs::Joy::ConstPtr& Joy);
    ros::NodeHandle n;
    ros::Subscriber sub ;
    ros::Publisher pub, pub_steer_z, pub_steer_y, pub_lifter;
    geometry_msgs::Twist twist;
    double v_x,v_y,v_angular;
    int active;
    int old_active;
    int _task[2];
    int tmp;
    int task_A;
    int task_B;
    int steer_gain;
    std_msgs::UInt16 steer_z;
    std_msgs::UInt16 steer_y;
    std_msgs::Float64 lifter;
};
 
Teleop::Teleop()
{

    n.getParam("/teleop_joy/w_speed_scale", v_angular);
    n.getParam("/teleop_joy/x_speed_scale", v_x);
    n.getParam("/teleop_joy/y_speed_scale", v_y);
    n.getParam("/teleop_joy/task_a", task_A);
    n.getParam("/teleop_joy/task_b", task_B);
    n.getParam("/teleop_joy/steer_gain", steer_gain);
    
    active = 0;
    old_active = 0;
    _task[0] = 0;
    _task[1] = 0;
    steer_z.data = 500;
    steer_y.data = 500;

    pub_steer_z = n.advertise<std_msgs::UInt16>("/steer_z",1);
    pub_steer_y = n.advertise<std_msgs::UInt16>("/steer_y",1);
    pub_lifter = n.advertise<std_msgs::Float64>("/lifter",1);

    pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

    sub = n.subscribe<sensor_msgs::Joy>("joy",10,&Teleop::callback,this);
    ros::Rate loop_rate(10);
    
    while(ros::ok())
    {
        if(active)
        {
            pub.publish(twist);
            old_active = 1;
        }
        else
        {
            if(old_active)
            {
                twist.linear.x = 0;
                twist.linear.y = 0;
                twist.angular.z = 0;
                pub.publish(twist);
            }
            old_active = active;
        }
        ros::spinOnce();
        loop_rate.sleep(); 
    }
}
 
void Teleop::callback(const sensor_msgs::Joy::ConstPtr& Joy)
{
    twist.linear.x =(Joy->axes[4])*v_x;
    twist.linear.y =(Joy->axes[3])*v_y;
    twist.angular.z = (Joy->axes[0])*v_angular;
    if(Joy->buttons[4] == 1)
    { 
        active = 1;
    }
    else
    {
        active = 0;
    }
    
    if(Joy->buttons[0] == 1)
    {
        _task[0] = 1;
        printf("[TELEOP]: 设置任务A[%d]\r\n", task_A);
    }

    if(Joy->buttons[1] == 1)
    {
        _task[1] = 1;
        printf("[TELEOP]: 设置任务B[%d]\r\n", task_B);
    }

    if(Joy->buttons[3] == 1)
    {
        if((_task[1]==0)&&(_task[0]==0))
        {
            printf("[TELEOP]: 请按[A B]键设置任务\r\n");
        }
        else
        {
            if(_task[0])
            {
                _task[0] = 0;
                n.getParam("/usr_task", tmp);
                if(task_A==tmp)
                {
                    printf("[TELEOP]: 任务A已经存在[%d]\r\n", task_A);
                }
                else
                {
                    printf("[TELEOP]: 执行任务A[%d]\r\n", task_A);
                    n.setParam("/usr_task", task_A);    
                }
            }

            if(_task[1])
            {
                _task[1] = 0;
                n.getParam("/usr_task", tmp);
                if(task_B==tmp)
                {
                    printf("[TELEOP]: 任务B已经存在[%d]\r\n", task_B);
                }
                else
                {
                    printf("[TELEOP]: 执行任务B[%d]\r\n", task_B);
                    n.setParam("/usr_task", task_B);    
                }
            }            
        }
    }
    
    if(Joy->buttons[2] == 1)
    {
        printf("[TELEOP]: 取消任务\r\n");
        tmp = 0;
        n.setParam("/usr_task", tmp); 
    }

    if(Joy->buttons[5] == 1)
    {
        //left,right
        if(Joy->axes[6] == 1)
        {
            if(steer_z.data>=1000)
            {
                steer_z.data = 1000;
            }
            else
            {
                steer_z.data += steer_gain;
            }
            pub_steer_z.publish(steer_z);
        }
        else if(Joy->axes[6] == -1)
        {
            if(steer_z.data<=0)
            {
                steer_z.data = 0;
            }
            else
            {
                steer_z.data -= steer_gain;
            }
            pub_steer_z.publish(steer_z); 
        }

        //up,down
        if(Joy->axes[7] == -1)
        {
            if(steer_y.data>=1000)
            {
                steer_y.data = 1000;
            }
            else
            {
                steer_y.data += steer_gain;
            }
            pub_steer_y.publish(steer_y);
        }
        else if(Joy->axes[7] == 1)
        {
            if(steer_y.data<=0)
            {
                steer_y.data = 0;
            }
            else
            {
                steer_y.data -= steer_gain;
            }
            pub_steer_y.publish(steer_y);
        }
    }
    else
    {
        if(Joy->axes[7] == 1)
        {
            if(lifter.data>=0.5)
            {
                lifter.data = 0.5;
            }
            else
            {
                lifter.data += 0.001;
            }
            pub_lifter.publish(lifter);
        }
        else if(Joy->axes[7] == -1)
        {
            if(lifter.data<=0)
            {
                lifter.data = 0;
            }
            else
            {
                lifter.data -= 0.001;
            }
            pub_lifter.publish(lifter);
        }
    } 
}
 
int main(int argc,char** argv)
{
    ros::init(argc, argv, "teleop_joy");
    Teleop telelop;
    return 0;
}
